import struct
import socket
import threading
from queue import Queue

DEFAULT_REMOTE_IP="192.168.2.136"
DEFAULT_REMOTE_PORT=50000
DEFAULT_HOST_IP="192.168.2.6"
DEFAULT_HOST_PORT=50000

class UData:
    def __init__(self, timestamp=0, pos=[0.0] * 6): #TODO:all zero initialization may cause danger
        self.timestamp = timestamp
        self.pos = pos

    def pack(self):
        return struct.pack('!Q6f', self.timestamp, *self.pos)

    @staticmethod
    def unpack(data):
        unpacked = struct.unpack('!Q6f', data)
        return UData(unpacked[0], list(unpacked[1:]))
    
    def __str__(self):
        return f"UData:timestamp={self.timestamp},pos={self.pos}"

class YData:
    def __init__(self, timestamp=0, pos=[0.0] * 6, vel=[0.0] * 6,F=[0.0] * 6):
        self.timestamp = timestamp
        self.pos = pos
        self.vel = vel
        self.F=F

    def pack(self):
        return struct.pack('!Q6f6f6f', self.timestamp, *self.pos, *self.vel,*self.F)

    @staticmethod
    def unpack(data):
        unpacked = struct.unpack('!Q6f6f6f', data)
        return YData(unpacked[0], list(unpacked[1:7]), list(unpacked[7:13]),list(unpacked[13:]))
    def __str__(self):
        return f"YData:timestamp={self.timestamp},pos={self.pos},vel={self.vel},F={self.F}"

class Enet:
    def __init__(self, rIP=DEFAULT_REMOTE_IP,rPort=DEFAULT_REMOTE_PORT,hIP=DEFAULT_HOST_IP,hPort=DEFAULT_HOST_PORT):
        self.remote_ip = rIP
        self.remote_port = rPort
        self.host_ip=hIP
        self.host_port=hPort
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.sock.bind((hIP, hPort))


        self.lock=threading.Lock()
        self.YPool=YData()
        # self.YPool = Queue(maxsize=1)

    def ReadY(self):
        with self.lock:
            return self.YPool
        

        # if not self.YPool.empty():
        #     return self.YPool.queue[-1]  # Access the last element directly from the queue's internal list
        # return None
        # return self.YPool

    def WriteY(self, y_data):
        with self.lock:
            self.YPool=y_data

        # if self.YPool.full():
        #     self.YPool.get()  # Remove the oldest element if the queue is full
        # self.YPool.put(y_data)

        # self.YPool=y_data

    def SendU(self, u_data):
        packed_data = u_data.pack()
        self.sock.sendto(packed_data, (self.remote_ip, self.remote_port))

    def ReceiveY(self):
        def listen():
            while True:
                data, _ = self.sock.recvfrom(1024)
                y_data = YData.unpack(data)
                #print(y_data)
                self.WriteY(y_data)

        thread = threading.Thread(target=listen, daemon=True)
        thread.start()
